package robocup.component;

import java.util.ArrayList;
import java.util.Iterator;

import robocup.component.actions.Action;
import robocup.component.actions.DashAction;
import robocup.component.actions.TurnAction;
import robocup.component.geometry.Vektor;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Player.RELATIVE;

public class AStarActionQueue {

  Player                     player;
  WorldModel                 world;
  ArrayList<OperationObject> aStarQueue = null;
  // private Vektor startPosition = null;
  Vektor                     lastGoal   = null;
  Vektor                     fcPos      = null;
  int                        restCycles = 0;

  public int getRestCycles() {

    return this.restCycles;
  }

  public AStarActionQueue(
      final WorldModel w,
      final Player p) {

    super();
    this.world = w;
    this.player = p;
  }

  public boolean calculateAStarQueue(final WorldModel w,
      final Player p,
      final Vektor goal,
      final int nrOfCycles) {

    this.world = w;
    this.player = p;
    return this.calculateAStarQueue(goal, nrOfCycles);
  }

  public boolean calculateAStarQueue(final Vektor goal,
      final int nrOfCycles) {

    this.restCycles = nrOfCycles;
    if (Math.abs(this.player.getDistance(goal)) > nrOfCycles) {
      return false;
    }
    if (Math
        .abs(((this.player.getDistance(goal) / nrOfCycles * 100) - 45) * nrOfCycles) >= this.player
        .getStamina()) {
      return false;
    }

    System.out.println("calculating new plan...");
    this.lastGoal = goal.cloned();
    OperationObject firstElement = new OperationObject(this.player.cloned());
    firstElement.setRest(firstElement.getPlayer().getDistance(goal));

    this.aStarQueue = new ArrayList<OperationObject>();
    this.aStarQueue.add(firstElement);

    while (firstElement.getRest() > 0.5) {
      final Vektor tmpPos = firstElement.getPlayer().getPosition().cloned();
      double tmpAngle = firstElement.getPlayer().getAngleRel(goal,
          RELATIVE.TO_BODY);
      tmpAngle = firstElement.getPlayer().getAngleForTurn(tmpAngle);
      final Action first = new TurnAction(tmpAngle);
      final Action second = new DashAction((int) ((firstElement.getPlayer()
          .getDistance(goal) / (nrOfCycles - firstElement.getOperationList()
          .size())) * 100));

      // Add TurnAction on firstElementClone
      if (Math.abs(tmpAngle) > 1) {
        final OperationObject newElement = new OperationObject(firstElement
            .getPlayer().predictPlayerAfterAction(first));
        newElement
            .setOperationList((ArrayList<OperationObject.ActionElement>) firstElement
                .getOperationList().clone());
        newElement.addAction(first);
        newElement.addToDeltaStart(newElement.getPlayer().getDistance(tmpPos));
        newElement.setRest(newElement.getPlayer().getDistance(goal));
        this.aStarQueue.add(newElement);
      }

      // Do DashAction on firstElement
      firstElement.getPlayer().predictThisAfterAction(second);
      firstElement
          .addToDeltaStart(firstElement.getPlayer().getDistance(tmpPos));
      firstElement.setRest(firstElement.getPlayer().getDistance(goal));
      firstElement.addAction(second);

      // Sort
      this.quickSort(this.aStarQueue, 0, this.aStarQueue.size() - 1);

      // getFirstElement
      firstElement = this.aStarQueue.get(0);
    }
    // debugOut(aStarQueue);
    return true;
  }

  public Action getNextAction() {

    if (this.aStarQueue.get(0).getOperationList().size() == 0) {
      return null;
    }
    final Action nextAction = this.aStarQueue.get(0).getOperationList().get(0)
        .getAction();
    this.fcPos = this.aStarQueue.get(0).getOperationList().get(0).getPosition();
    this.aStarQueue.get(0).getOperationList().remove(0);
    this.restCycles = this.restCycles - 1;
    return nextAction;
  }

  public boolean nextActionAvailable() {

    if (this.aStarQueue == null) {
      return false;
    }
    if (this.aStarQueue.get(0).getOperationList().size() == 0) {
      return false;
    }
    return true;
  }

  public double deviationControl(final Vektor actPos) {

    return this.fcPos.getDistance(actPos);
  }

  private void quickSort(final ArrayList<OperationObject> list,
      final int low0,
      final int high0) {

    int low = low0, high = high0;
    if (low >= high) {
      return;
    }
    else if (low == high - 1) {
      if ((list.get(low).getWeight()) > (list.get(high).getWeight())) {
        final OperationObject temp = list.get(low);
        list.set(low, list.get(high));
        list.set(high, temp);
      }
      return;
    }

    final OperationObject pivot = list.get((low + high) / 2);
    list.set((low + high) / 2, list.get(high));
    list.set(high, pivot);

    while (low < high) {
      while ((list.get(low).getWeight()) <= (pivot.getWeight()) && low < high) {
        low++;
      }
      while ((list.get(high).getWeight()) >= (pivot.getWeight()) && low < high) {
        high--;
      }
      if (low < high) {
        final OperationObject temp = list.get(low);
        list.set(low, list.get(high));
        list.set(high, temp);
      }
    }
    list.set(high0, list.get(high));
    list.set(high, pivot);
    this.quickSort(list, low0, low - 1);
    this.quickSort(list, high + 1, high0);
  }

  private void debugOut(final ArrayList<OperationObject> a) {

    final Iterator it = a.iterator();
    Iterator it2;
    int i;

    while (it.hasNext()) {
      final OperationObject o = (OperationObject) it.next();
      System.out.println("Gone so far: " + o.getDeltaStart() + "   Goal: " + o
          .getRest() + "   Weight: " + o.getWeight());
      it2 = o.getOperationList().iterator();
      System.out.println(it2.hasNext());
      i = 0;
      while (it2.hasNext()) {
        final OperationObject.ActionElement ae = (OperationObject.ActionElement) it2
            .next();
        System.out
            .println("Action" + i + ": " + ae.getAction().toString() + " Position: " + ae
                .getPosition());
        i++;
      }
    }
    System.out.println(" ");
  }

  private class OperationObject {

    // Constructor
    private OperationObject(
        final Player p) {

      this.virtualPlayer = p;
    }

    // ClassVars
    ArrayList<ActionElement> operationList = new ArrayList<ActionElement>();
    Player                   virtualPlayer = null;
    private double           weight        = 0;
    private double           deltaStart    = 0;
    private double           goalDist      = 1;

    // Properties
    public Player getPlayer() {

      return this.virtualPlayer;
    }

    public double getDeltaStart() {

      return this.deltaStart;
    }

    public void addToDeltaStart(final double value) {

      this.deltaStart += value;
    }

    public double getWeight() {

      return this.weight;
    }

    public double getRest() {

      return this.goalDist;
    }

    public void setRest(final double value) {

      this.goalDist = value;
      this.weight = this.goalDist + this.deltaStart;
    }

    public ArrayList<ActionElement> getOperationList() {

      return this.operationList;
    }

    public void setOperationList(final ArrayList<ActionElement> listCopy) {

      this.operationList = listCopy;
    }

    public void addAction(final Action thisAction) {

      this.operationList.add(new ActionElement(thisAction, this.virtualPlayer
          .getPosition().cloned()));
    }

    private class ActionElement {

      private Action forecastAction;
      private Vektor forecastPosition;

      private ActionElement(
          final Action a,
          final Vektor v) {

        this.forecastAction = a;
        this.forecastPosition = v;
      }

      public Action getAction() {

        return this.forecastAction;
      }

      public Vektor getPosition() {

        return this.forecastPosition;
      }
    }
  }
}
